/**
 * Author: luoqi
 * Created Date: 2025-10-14 00:05:21
 * Last Modified: 2025-11-26 00:37:6
 * Modified By: luoqi at <**@****>
 * Copyright (c) 2025 <*****>
 * Description:
 */

#include <algorithm>
#include <utils/log.hpp>
#include "powerflow.h"

PowerFlow::PowerFlow()
{
    joints_.resize(7);
    for(int i = 0; i < joints_.size(); i++) {
        joints_[i].id = 0x0001 + i;
        mit_pack_(mit_msgs_[i], 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
    }
}

PowerFlow::~PowerFlow()
{

}


int PowerFlow::port_set(SendPort send, RecvPort recv)
{
    this->send_ = send;
    this->recv_ = recv;
    return 0;
}

int PowerFlow::fp2uint_(float x, float min, float max, int bits)
{
    // Converts a float to an unsigned   int, given range and number of bits
    float span = max - min;
    float offset = min;
    return (int)((x - offset) * ((float)((1 << bits) - 1)) / span);
}

float PowerFlow::uint2fp_(int x, float min, float max, int bits)
{
    // Converts an unsigned int to a float, given range and number of bits
    float span = max - min;
    float offset = min;
    return (float)x * span / ((float)((1 << bits) - 1)) + offset;
}

int PowerFlow::decode(uint16_t id, void *data, size_t sz)
{
    if(sz != 8) {
        return -1;
    }
    uint8_t *d = (uint8_t *)data;
    for(auto &q : joints_) {
        if(q.id == id) {
            int _pos = ((uint16_t)d[0] << 8) | d[1];
            int _vel = (((uint16_t)d[2] << 4) & 0x0ff0) | ((d[3] >> 4) & 0x0f);
            int _cur = (((uint16_t)d[3] << 8) & 0x0f00) | (d[4] & 0xff);
            int _err = (d[5] >> 4) & 0x0f;
            int _state = (d[4] >> 1) & 0x07;
            int _hb = d[5] & 0x01;
            q.pos = uint2fp_(_pos, -6.28f, 6.28f, 16);
            q.vel = uint2fp_(_vel, -12.56f, 12.56f, 12);
            if(q.type == R56) {
                q.cur = uint2fp_(_cur, -50.0f, 50.0f, 12);
            } else {
                q.cur = uint2fp_(_cur, -100.0f, 100.0f, 12);
            }
            if(_err == 0) {
                q.err = NONE;
            } else {
                q.err = (ErrCode)std::pow(2, _err);
            }
            q.state = (State)_state;
            q.hb = (_hb == 0x01);
        }
    }
    return 0;
}

void PowerFlow::mit_pack_(MitMsg &frame, float pos, float vel, float tor, float kp, float kd)
{
    int _pos = fp2uint_(pos, -6.28f, 6.28f, 16);
    int _vel = fp2uint_(vel, -12.56f, 12.56f, 12);
    int _tor = fp2uint_(tor, -50.0f, 50.0f, 12);
    int _kp = fp2uint_(kp, 0.0f, 500.0f, 12);
    int _kd = fp2uint_(kd, 0.0f, 8.0f, 12);

    frame[0] = (_pos >> 8) & 0xff;
    frame[1] = _pos & 0xff;
    frame[2] = (_vel >> 4) & 0xff;
    frame[3] = ((_vel & 0x0f) << 4) | ((_kp >> 8) & 0xff);
    frame[4] = (_kp & 0xff);
    frame[5] = ((_tor >> 4) & 0xff);
    frame[6] = ((_kd & 0x0f) << 4) | ((_tor >> 8) & 0xff);
    frame[7] = _tor & 0xff;
}


int PowerFlow::enable(int joint)
{
    if((joint < 1) || (joint > joints_.size()) || (send_ == nullptr)) {
        return -1;
    }

    uint8_t data[2] = { 0x01, 0x01 };
    return send_(joints_[joint - 1].id, data, sizeof(data));
}

int PowerFlow::disable(int joint)
{
    if((joint < 1) || (joint > joints_.size()) || (send_ == nullptr)) {
        return -1;
    }

    uint8_t data[2] = { 0x01, 0x00 };
    return send_(joints_[joint - 1].id, data, sizeof(data));
}

int PowerFlow::mode_set(int joint, Mode mode)
{
    if((joint < 0) || (joint >= joints_.size()) || (send_ == nullptr)) {
        return -1;
    }

    return 0;
}

int PowerFlow::cur_set(int joint, float cur)
{
    if((joint < 1) || (joint > joints_.size()) || (send_ == nullptr)) {
        return -1;
    }

    return 0;
}

int PowerFlow::vel_set(int joint, float vel)
{
    if((joint < 1) || (joint > joints_.size()) || (send_ == nullptr)) {
        return -1;
    }

    return 0;
}

int PowerFlow::pos_set(int joint, float pos)
{
    if((joint < 1) || (joint > joints_.size()) || (send_ == nullptr)) {
        return -1;
    }

    return 0;
}

int PowerFlow::mit_set(int joint, float pos, float vel, float tor, float kp, float kd)
{
    if((joint < 1) || (joint > joints_.size()) || (send_ == nullptr)) {
        return -1;
    }
    pos = std::clamp(pos, -6.28f, 6.28f);
    vel = std::clamp(vel, -12.56f, 12.56f);
    if(joints_[joint - 1].type == R56) {
        tor = std::clamp(tor, -50.0f, 50.0f);
    } else {
        tor = std::clamp(tor, -100.0f, 100.0f);
    }
    kp = std::clamp(kp, 0.0f, 500.0f);
    kd = std::clamp(kd, 0.0f, 8.0f);

    std::lock_guard<std::mutex> lock(mtx_);
    mit_pack_(mit_msgs_[joint - 1], pos, vel, tor, kp, kd);

    return send_(0x0000, mit_msgs_, sizeof(mit_msgs_));
}

// int PowerFlow::id_set(int joint, uint16_t id)
// {
//     if((joint < 1) || (joint > joints_.size()) || (send == nullptr)) {
//         return -1;
//     }

//     return 0;
// }

// int PowerFlow::zero_set(int joint)
// {
//     if((joint < 1) || (joint > joints_.size()) || (send == nullptr)) {
//         return -1;
//     }

//     return 0;
// }

// int PowerFlow::config_save(int joint)
// {
//     if((joint < 1) || (joint > joints_.size()) || (send == nullptr)) {
//         return -1;
//     }

//     return 0;
// }

int PowerFlow::recv_decode(int timeout_ms)
{
    if(recv_ == nullptr) {
        return -1;
    }
    uint16_t id;
    uint8_t sz;
    uint8_t data[64];
    int nbytes = recv_(id, data, sz, timeout_ms);
    if(nbytes < 0) {
        QLog::err("PowerFlow::recv_decode: recv failed");
        return -1;
    } else if(nbytes == 0) {
        QLog::err("PowerFlow::recv_decode: recv timeout");
        return 0;
    } else{
        if(decode(id, data, sz) < 0) {
            QLog::err("PowerFlow::recv_decode: decode failed, id: 0x{:04x}, sz: {}", id, sz);
        }
    }

    return 0;
}

int PowerFlow::poll()
{
    if(send_ == nullptr) {
        return -1;
    }

    std::lock_guard<std::mutex> lock(mtx_);
    return send_(0x0000, mit_msgs_, sizeof(mit_msgs_));
}
